////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//                                                                                                                            //
//                                                         WNProject                                                          //
//                                                                                                                            //
//            This file is distributed under the BSD 2-Clause open source license. See License.txt for details.               //
//                                                                                                                            //
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#pragma once

#ifndef __WN_MATH_QUATERNION_INL__
#define __WN_MATH_QUATERNION_INL__

#ifndef __WN_MATH_QUATERNION_H__
    #error "WNQuaternion.inl should never be included directly. Please include WNQuaternion.h instead"
#endif

#include "WNMath/inc/WNBasic.h"
#include "WNMath/inc/WNMatrix3.h"
#include "WNMath/inc/WNMatrix4.h"
#include "WNMath/inc/WNVector4.h"

namespace WNMath {
    template <typename Type>
    WNQuaternion<Type>::WNQuaternion(const WNQuaternion<Type>& _quaternion) {
        Set(_quaternion);
    }

    template <typename Type>
    WNQuaternion<Type>::WNQuaternion(Type _number) {
        Set(_number);
    }

    template <typename Type>
    WNQuaternion<Type>::WNQuaternion(const Type* _numbers) {
        Set(_numbers);
    }

    template <typename Type>
    WNQuaternion<Type>::WNQuaternion(Type _x, Type _y, Type _z, Type _w) {
        Set(_x, _y, _z, _w);
    }

    template <typename Type>
    WNQuaternion<Type>::operator Type* () {
        return(&this->mX);
    }

    template <typename Type>
    WNQuaternion<Type>::operator const Type* () const {
        return(&this->mX);
    }

    template <typename Type>
    WNQuaternion<Type>& WNQuaternion<Type>::operator += (const WNQuaternion<Type>& _quaternion) {
        this->mX += _quaternion.mX;
        this->mY += _quaternion.mY;
        this->mZ += _quaternion.mZ;
        this->mW += _quaternion.mW;

        return(*this);
    }

    template <typename Type>
    WNQuaternion<Type>& WNQuaternion<Type>::operator -= (const WNQuaternion<Type>& _quaternion) {
        this->mX -= _quaternion.mX;
        this->mY -= _quaternion.mY;
        this->mZ -= _quaternion.mZ;
        this->mW -= _quaternion.mW;

        return(*this);
    }

    template <typename Type>
    WNQuaternion<Type>& WNQuaternion<Type>::operator *= (const WNQuaternion<Type>& _quaternion) {
        const WNQuaternion<Type> quaternion = *this;

        this->mX = quaternion.mW * _quaternion.mX + quaternion.mX * _quaternion.mW +
                   quaternion.mY * _quaternion.mZ - quaternion.mZ * _quaternion.mY;
        this->mY = quaternion.mW * _quaternion.mY + quaternion.mY * _quaternion.mW +
                   quaternion.mZ * _quaternion.mX - quaternion.mX * _quaternion.mZ;
        this->mZ = quaternion.mW * _quaternion.mZ + quaternion.mZ * _quaternion.mW +
                   quaternion.mX * _quaternion.mY - quaternion.mY * _quaternion.mX;
        this->mW = quaternion.mW * _quaternion.mW - quaternion.mX * _quaternion.mX -
                   quaternion.mY * _quaternion.mY - quaternion.mZ * _quaternion.mZ;

        return(*this);
    }

    template <typename Type>
    WNQuaternion<Type>& WNQuaternion<Type>::operator *= (Type _number) {
        this->mX *= _number;
        this->mY *= _number;
        this->mZ *= _number;
        this->mW *= _number;

        return(*this);
    }

    template <typename Type>
    WNQuaternion<Type>& WNQuaternion<Type>::operator /= (Type _number) {
        this->mX /= _number;
        this->mY /= _number;
        this->mZ /= _number;
        this->mW /= _number;

        return(*this);
    }

    template <typename Type>
    WNQuaternion<Type> WNQuaternion<Type>::operator + () const {
        return(*this);
    }

    template <typename Type>
    WNQuaternion<Type> WNQuaternion<Type>::operator - () const {
        return(WNQuaternion<Type>(-this->mX, -this->mY, -this->mZ, -this->mW));
    }

    template <typename Type>
    WNQuaternion<Type> WNQuaternion<Type>::operator + (const WNQuaternion<Type>& _quaternion) const {
        return(WNQuaternion<Type>(this->mX + _quaternion.mX, this->mY + _quaternion.mY, this->mZ + _quaternion.mZ, this->mW + _quaternion.mW));
    }

    template <typename Type>
    WNQuaternion<Type> WNQuaternion<Type>::operator - (const WNQuaternion<Type>& _quaternion) const {
        return(WNQuaternion<Type>(this->mX - _quaternion.mX, this->mY - _quaternion.mY, this->mZ - _quaternion.mZ, this->mW - _quaternion.mW));
    }

    template <typename Type>
    WNQuaternion<Type> WNQuaternion<Type>::operator * (const WNQuaternion<Type>& _quaternion) const {
        return(WNQuaternion<Type>(this->mW * _quaternion.mX + this->mX * _quaternion.mW +
                                  this->mY * _quaternion.mZ - this->mZ * _quaternion.mY,
                                  this->mW * _quaternion.mY + this->mY * _quaternion.mW +
                                  this->mZ * _quaternion.mX - this->mX * _quaternion.mZ,
                                  this->mW * _quaternion.mZ + this->mZ * _quaternion.mW +
                                  this->mX * _quaternion.mY - this->mY * _quaternion.mX,
                                  this->mW * _quaternion.mW - this->mX * _quaternion.mX -
                                  this->mY * _quaternion.mY - this->mZ * _quaternion.mZ));
    }

    template <typename Type>
    WNQuaternion<Type> WNQuaternion<Type>::operator * (Type _number) const {
        return(WNQuaternion<Type>(this->mX * _number, this->mY * _number, this->mZ * _number, this->mW * _number));
    }

    template <typename Type>
    WNQuaternion<Type> WNQuaternion<Type>::operator / (Type _number) const {
        return(WNQuaternion<Type>(this->mX / _number, this->mY / _number, this->mZ / _number, this->mW / _number));
    }

    template <typename Type>
    WN_BOOL WNQuaternion<Type>::operator == (const WNQuaternion<Type>& _quaternion) const {
        return(this->mX == _quaternion.mX && this->mY == _quaternion.mY && this->mZ == _quaternion.mZ && this->mW == _quaternion.mW);
    }

    template <typename Type>
    WN_BOOL WNQuaternion<Type>::operator != (const WNQuaternion<Type>& _quaternion) const {
        return(this->mX != _quaternion.mX || this->mY != _quaternion.mY || this->mZ != _quaternion.mZ || this->mW != _quaternion.mW);
    }

    template <typename Type>
    WN_VOID WNQuaternion<Type>::Zero() {
        this->mX = static_cast<Type>(0);
        this->mY = static_cast<Type>(0);
        this->mZ = static_cast<Type>(0);
        this->mW = static_cast<Type>(0);
    }

    template <typename Type>
    WN_VOID WNQuaternion<Type>::Identity() {
        this->mX = static_cast<Type>(0);
        this->mY = static_cast<Type>(0);
        this->mZ = static_cast<Type>(0);
        this->mW = static_cast<Type>(1);
    }

    template <typename Type>
    WN_VOID WNQuaternion<Type>::Normalize() {
        const Type length = Length();

        *this /= length;
    }

    template <typename Type>
    WN_VOID WNQuaternion<Type>::Invert() {
        *this = GetConjugate();
        Normalize();
    }

    template <typename Type>
    WN_VOID WNQuaternion<Type>::Logarithm() {
        if (WNAbs(this->mW) <= static_cast<Type>(1)) {
            const Type theta = WNACos(this->mW);
            const Type thetaOverSinTheta  = theta / WNSin(theta);

            this->mX *= thetaOverSinTheta;
            this->mY *= thetaOverSinTheta;
            this->mZ *= thetaOverSinTheta;
        }

        this->mW = static_cast<Type>(0);
    }

    template <typename Type>
    WN_VOID WNQuaternion<Type>::Exponent() {
        const WNQuaternion<Type> quaternion = *this;
        const Type theta = WNSqrt(quaternion.mX * quaternion.mX + quaternion.mY * quaternion.mY + quaternion.mZ * quaternion.mZ);
        const Type cosTheta = WNCos(theta);

        if (WNAbs(theta) >= static_cast<Type>(0)) {
            const Type oneOverTheta = static_cast<Type>(1) / theta;
            const Type sinTheta = WNSin(theta);

            this->mX = quaternion.mX * oneOverTheta * sinTheta;
            this->mY = quaternion.mY * oneOverTheta * sinTheta;
            this->mZ = quaternion.mZ * oneOverTheta * sinTheta;
        }

        this->mW = cosTheta;
    }

    template <typename Type>
    WN_VOID WNQuaternion<Type>::Rotate(const WNQuaternion<Type>& _rotation) {
        *this *= _rotation;
    }

    template <typename Type>
    Type WNQuaternion<Type>::Length() const {
        const WNQuaternion<Type> quaternion = *this;

        return(WNSqrt((quaternion.mX * quaternion.mX) + (quaternion.mY * quaternion.mY) +
                      (quaternion.mZ * quaternion.mZ) + (quaternion.mW * quaternion.mW)));
    }

    template <typename Type>
    Type WNQuaternion<Type>::LengthSquared() const {
        const WNQuaternion<Type> quaternion = *this;

        return((quaternion.mX * quaternion.mX) + (quaternion.mY * quaternion.mY) +
               (quaternion.mZ * quaternion.mZ) + (quaternion.mW * quaternion.mW));
    }

    template <typename Type>
    Type WNQuaternion<Type>::Dot(const WNQuaternion<Type>& _quaternion) const {
        const WNQuaternion<Type> quaternion = *this;

        return((quaternion.mX * _quaternion.mX) + (quaternion.mY * _quaternion.mY) +
               (quaternion.mZ * _quaternion.mZ) + (quaternion.mW * _quaternion.mW));
    }

    template <typename Type>
    WN_BOOL WNQuaternion<Type>::IsZero() const {
        return(this->mX == static_cast<Type>(0) && this->mY == static_cast<Type>(0) &&
               this->mZ == static_cast<Type>(0) && this->mW == static_cast<Type>(0));
    }

    template <typename Type>
    WN_BOOL WNQuaternion<Type>::IsIdentity() const {
        return(this->mX == static_cast<Type>(0) && this->mY == static_cast<Type>(0) &&
               this->mZ == static_cast<Type>(0) && this->mW == static_cast<Type>(1));
    }

    template <typename Type>
    WN_VOID WNQuaternion<Type>::Set(const WNQuaternion<Type>& _quaternion) {
        *this = _quaternion;
    }

    template <typename Type>
    WN_VOID WNQuaternion<Type>::Set(const Type* _numbers) {
        this->mX = _numbers[0];
        this->mY = _numbers[1];
        this->mZ = _numbers[2];
        this->mW = _numbers[3];
    }

    template <typename Type>
    WN_VOID WNQuaternion<Type>::Set(Type _number) {
        this->mX = _number;
        this->mY = _number;
        this->mZ = _number;
        this->mW = _number;
    }

    template <typename Type>
    WN_VOID WNQuaternion<Type>::Set(Type _x, Type _y, Type _z, Type _w) {
        this->mX = _x;
        this->mY = _y;
        this->mZ = _z;
        this->mW = _w;
    }

    template <typename Type>
    WNQuaternion<Type> WNQuaternion<Type>::GetNormalized() const {
        WNQuaternion<Type> quaternion = *this;

        return(quaternion.Normalize(), quaternion);
    }

    template <typename Type>
    WNQuaternion<Type> WNQuaternion<Type>::GetInverted() const {
        WNQuaternion<Type> quaternion = *this;

        return(quaternion.Invert(), quaternion);
    }

    template <typename Type>
    WNQuaternion<Type> WNQuaternion<Type>::GetConjugate() const {
        WNQuaternion<Type> quaternion = *this;

        quaternion.mX = -quaternion.mX;
        quaternion.mY = -quaternion.mY;
        quaternion.mZ = -quaternion.mZ;

        return(quaternion);
    }

    template <typename Type>
    WNQuaternion<Type> WNQuaternion<Type>::GetLogarithm() const {
        WNQuaternion<Type> quaternion = *this;

        return(quaternion.Logarithm(), quaternion);
    }

    template <typename Type>
    WNQuaternion<Type> WNQuaternion<Type>::GetExponent() const {
        WNQuaternion<Type> quaternion = *this;

        return(quaternion.Exponent(), quaternion);
    }

    template <typename Type>
    WNMatrix4<Type> WNQuaternion<Type>::ToMatrix44() const {
        const Type x2 = this->mX + this->mX;
        const Type y2 = this->mY + this->mY;
        const Type z2 = this->mZ + this->mZ;
        const Type wx = this->mW * x2;
        const Type wy = this->mW * y2;
        const Type wz = this->mW * z2;
        const Type xx = this->mX * x2;
        const Type yy = this->mY * y2;
        const Type yz = this->mY * z2;
        const Type xy = this->mX * y2;
        const Type xz = this->mX * z2;
        const Type zz = this->mZ * z2;

        return(WNMatrix4<Type>(static_cast<Type>(1) - (yy + zz), xy - wz, xz + wy, static_cast<Type>(0),
                                xy + wz, static_cast<Type>(1) - (xx + zz), yz - wx, static_cast<Type>(0),
                                xz - wy, yz + wx, static_cast<Type>(1) - (xx + yy), static_cast<Type>(0),
                                static_cast<Type>(0), static_cast<Type>(0), static_cast<Type>(0), static_cast<Type>(1)));
    }

    #ifndef __WN_MATH_HAS_QUATERNION_CUSTOM_PREFETCH
        template <typename Type>
        WN_VOID WNQuaternion<Type>::Prefetch() const {
            // Default
        }
    #endif

    template <typename Type>
    template <typename NewType>
    WNQuaternion<NewType> WNQuaternion<Type>::ConvertTo() const {
        return(WNQuaternion<NewType>(static_cast<NewType>(this->mX), static_cast<NewType>(this->mY),
                                     static_cast<NewType>(this->mZ), static_cast<NewType>(this->mW)));
    }

    template <typename Type>
    WNQuaternion<Type> operator * (Type _number, const WNQuaternion<Type>& _quaternion) {
        WNQuaternion<Type> quaternion = _quaternion;

        quaternion *= _number;

        return(quaternion);
    }

    template <typename Type>
    WNQuaternion<Type> operator / (Type _number, const WNQuaternion<Type>& _quaternion) {
        return(WNQuaternion<Type>(_number / _quaternion.mX,
                                  _number / _quaternion.mY,
                                  _number / _quaternion.mZ,
                                  _number / _quaternion.mW));
    }
}

#ifdef __WN_MATH_ENABLE_EXTENSIONS
    #ifdef _WN_X86
        //#include "WNMath/inc/Internal/x86/WNQuaternion.inl"
    #endif
#endif

#endif // __WN_MATH_QUATERNION_2_INL__